#pragma once
#include "tools/DataLoder.h"
#include <opencv2/core/eigen.hpp>
#include <opencv2/opencv.hpp>
#include <cuda.h>
#include <cuda_runtime.h>
#include "cublas_v2.h"
#include <cusolverDn.h>

#include "DEM.h"
#include "tools/OmpHelper.h"


class NRreger
{

private:
    MatrixXX mat_allp;
    SparseMatrix mat_M;
    size_t nframe = 0;//融合帧数
    
    /*优化参数相关*/
    int n_warp_num_;//(m)
    int n_corr_num_;//(n)
    int n_allpoint_num;//(a)

    int ntps_;//tps控制点数量

   
    //device和host 参数
    cudaError_t cudaStat_ ;
    cublasStatus_t stat_ ; 
    cublasHandle_t handle_ ;
    double* f_; double* f_d;   
    double* x_; double* x_d;
    double* s_t_; double* s_t_d;
    double* e_w_; double* e_w_d;
    double* m_; double* m_d;
    double* ap_tps_;double* ap_tps_d;

    double* X_;//tps的解向量
    double *deformap,*initialap;//所有点的初始位置和形变位置
    double *grad_;//梯度,这里直接取负梯度

private:

    /*原始数据相关*/
    cv::Mat dimg_src_;
    cv::Mat dimg_tar_;
    cv::Mat cimg_src_;       
    cv::Mat cimg_tar_;

    VPairs  correspondence_pairs_;

    /*几何相关*/
    PointCloud pc_DEM_tar_;
    PointCloud pc_tps;


private:

    /*其余指针*/
    DataLoder*       dataloder_;
    Paras            pars_;
    DEM*             dem_main_;//***************一直维护的DEM地图****************
    DEM*             dem_tar_; //瞬时地图

public:
    NRreger();
    NRreger(DataLoder* data,Paras paras);
    virtual int DoNonRigid();
    ~NRreger();
    void    Initial_GPU(int t);
    Scalar  sample_gradient_GPU(Scalar &data_err, Scalar &smooth_err,Scalar &tps_err,bool calgrad);//一个函数完成能量和梯度的计算
    int     DecentSolver_GPU(Scalar &data_err, Scalar &smooth_err,Scalar &tps_err);
    open3d::geometry::PointCloud  Shower_GPU();
    
private:
    
    void Clear();                    //清空数据
    Eigen::Vector3d FlowCorrespondence(VPairs & corres,std::string flowpath);
    void TPSUpdata_GPU(Scalar R);  
    void TPS_GPU(PointCloud pc_tps); //gpu计算tps参数
    void InitialX();

};
